#include <ros/ros.h>

#include "cg_interfaces/BridgeControl.h"
#include "cg_interfaces/BridgeSensor.h"
#include "cg_interfaces/ChassisControl.h"
#include "cg_interfaces/ChassisSensor.h"
#include "cg_interfaces/ChassisStruct.h"

struct ChassisConfig
{
    int bridgeCount;
    int masterId;
    int selfId;
    int struct_ids[4];

    ChassisConfig()
    {
        bridgeCount = 4;
        masterId = 1;
        struct_ids[0] = 1;
        struct_ids[1] = 0;
        struct_ids[2] = 0;
        struct_ids[3] = 0;
        selfId = -1;
    }

    bool LoadFromParam()
    {
        ros::NodeHandle nh;
        if (!nh.getParam("self", selfId))
        {
            ROS_ERROR("Failed to get self id from param");
            return false;
        }
    }
};

ros::Publisher bridge_control_pub[4];
ros::Subscriber bridge_sensor_sub[4];
ros::Subscriber chassis_struct_sub;
ros::Publisher chassis_sensor_pub;
ros::Subscriber chassis_control_sub;
ros::Timer timer;
ChassisConfig config;
cg_interfaces::BridgeSensor bridge_sensor_msg[4];
bool inMasterMode = false;

void OnChassisStructChanged(const cg_interfaces::ChassisStruct::ConstPtr &msg)
{
    // update local info
    config.bridgeCount = msg->count;
    config.masterId = msg->master;
    for (int i = 0; i < msg->count; i++)
    {
        config.struct_ids[i] = msg->struct_ids[i];
    }
    if (config.masterId == config.selfId)
    {
        timer.start();
        inMasterMode = true;
    }
    else
    {
        timer.stop();
        inMasterMode = false;
    }
}

void OnChassisControlChanged(const cg_interfaces::ChassisControl::ConstPtr &msg)
{
    if (!inMasterMode)
    {
        return;
    }
    if (msg->count != config.bridgeCount)
    {
        ROS_INFO("chassis control bridge count not match, ignore this message");
        return;
    }
    cg_interfaces::BridgeControl bridge_msg;
    for (int i = 0; i < msg->count; i++)
    {
        int id = config.struct_ids[i];
        int index = id - 1;
        bridge_msg = msg->bridges[i];
        bridge_msg.id = id;
        bridge_control_pub[index].publish(bridge_msg);
    }
}

void OnBridgeSensorChanged(const cg_interfaces::BridgeSensor::ConstPtr &msg, int id)
{
    bridge_sensor_msg[id - 1] = *msg;
}
void OnSensorTimer(const ros::TimerEvent &event)
{
    cg_interfaces::ChassisSensor sensor_msg;
    sensor_msg.bridge_count = config.bridgeCount;
    for (int i = 0; i < config.bridgeCount; i++)
    {
        int id = config.struct_ids[i];
        sensor_msg.bridges[i] = bridge_sensor_msg[id - 1];
    }
    chassis_sensor_pub.publish(sensor_msg);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "reconstruct_chassis");
    config.LoadFromParam();

    for (int i = 0; i < config.bridgeCount; i++)
    {
        int id = i + 1;
        ros::NodeHandle nh;
        std::string topic = "bridge" + std::to_string(id) + '/';
        bridge_control_pub[i] = nh.advertise<cg_interfaces::BridgeControl>(topic + "control", 1);
        bridge_sensor_sub[i] = nh.subscribe<cg_interfaces::BridgeSensor>(
            topic + "sensor", 1, std::bind(OnBridgeSensorChanged, std::placeholders::_1, id));
    }
    ros::NodeHandle nh;

    chassis_struct_sub = nh.subscribe<cg_interfaces::ChassisStruct>("chassis_struct", 1, OnChassisStructChanged);
    chassis_sensor_pub = nh.advertise<cg_interfaces::ChassisSensor>("chassis_sensor", 1);
    chassis_control_sub = nh.subscribe<cg_interfaces::ChassisControl>("chassis_control", 1, OnChassisControlChanged);
    timer = nh.createTimer(ros::Duration(1.0 / 50), OnSensorTimer);
    timer.stop();

    ros::spin();
    return 0;
}